#include"Armor.hpp"



/*
// 装甲板姿态
void Armor::armorPose(Mat &img){
    
    // 对于我们而言，也就是 照片上 装甲板 4个点的坐标。
    // 请你填写下面的 img_points:
    std::vector<cv::Point2f> img_points{ 
        this->left.topPoint,
        this->right.topPoint,
        this->right.bottomPoint,
        this->left.bottomPoint };

    Mat rvec, tvec;
    // 所有要传入的值都已经具备了。现在调用 solvePnP 解算装甲板位姿，
    // rvec 和 tvec 用于存储 solvePnP 输出的结果。
    // 你需要在下面填写 输入给 solvePnP 的参数：
    //
    solvePnP(object_points, img_points,camera_matrix ,distort_coeffs , rvec, tvec,false,SOLVEPNP_EPNP);// SOLVEPNP_IPPE 为什么加上这句话之后就使用不了了？
    // tools::draw_text(img, fmt::format("tvec:  x{: .2f} y{: .2f} z{: .3f}", tvec.at<double>(0),tvec.at<double>(1), tvec.at<double>(2)), cv::Point(10, 60), 1, cv::Scalar(0, 255, 255), 3);
    // tools::draw_text(img, fmt::format("rvec:  x{: .2f} y{: .2f} z{: .2f}", rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2)), cv::Point(10, 100), 1, cv::Scalar(0, 255, 255), 3);
    this->armorDistance = tvec.at<double>(2);// 距离
    this->rvec = rvec.clone();// 深拷贝构造
    this->tvec = tvec.clone();
    


    // 先将 Mat 矩阵转成 Vec3d 向量
    cv::Vec3d tvec_vec(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
    cv::Vec3d rvec_vec(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2));
    // 旋转矩阵 R 的列向量代表了物体坐标系的轴在相机坐标系中的方向
    cv::Mat R; // 旋转矩阵
    cv::Rodrigues(rvec_vec,R); // 旋转向量→旋转矩阵（3x3矩阵）得到法向量
    double diameter = 0.042; // 大概直径
    // 3. 提取红色面的法向量（单位向量）
    // 旋转矩阵的第3列 [R.at<double>(0,2), R.at<double>(1,2), R.at<double>(2,2)]
    // 即代表红色面在相机坐标系下的正方向法向量
    Vec3d Armor_normal(
        R.at<double>(0, 2),  // 法向量X分量
        R.at<double>(1, 2),  // 法向量Y分量
        R.at<double>(2, 2)   // 法向量Z分量
    );
    
    // 装甲板面 + 直径 × 法向量
    Vec3d virtualArmor_tvec = tvec_vec + diameter * Armor_normal;
    Vec3d virtualArmor_rvec = rvec_vec;



    // 3D 模型原点是装甲板中心 这个就是标定好的相机坐标系
    Point3f armor_center_3d(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
    // 1. 将virtualArmor_tvec转换为Point3f类型
    cv::Point3f virtual_armor_3d(virtualArmor_tvec[0], virtualArmor_tvec[1], virtualArmor_tvec[2]);
    
    // 将三维点转换成二维点
    vector<Point3f>point3d = {armor_center_3d,virtual_armor_3d};
    vector<Point2f>point2d;
    
    // 相机坐标系到图像的投影无需旋转和平移（传空矩阵）
    // 三维点转二维点
    projectPoints(
        point3d, 
        Mat::zeros(3, 1, CV_64F),  // 旋转向量（空）
        // rvec,
        Mat::zeros(3, 1, CV_64F),  // 平移向量（空）
        // tvec,
        camera_matrix, 
        distort_coeffs, 
        point2d
    );

    this->firePoint = point2d[1];

    // 二维图像位置
    circle(img,point2d[0],5,Scalar(0,205,255),-1);// 这个三维点转二维的点相对来说就是两条线段相交的点
    // tools::draw_text(img, fmt::format("{: .2f} {: .2f} {: .2f} ", point2d[1].x, point2d[1].y), Point2f(point2d[1].x, point2d[1].y), 1.7, Scalar(250, 255, 0), 3);
    cout<<point2d[1].x<<","<<point2d[1].y<<"point[1]"<<endl;
    cout<<point2d[0].x<<","<<point2d[0].y<<endl;
    cout<<endl;
    // 点的三维信息
    // tools::draw_text(img, fmt::format("{: .2f} {: .2f} {: .2f} ", point3d[0].x, point3d[0].y,point3d[0].z), Point2f(point2d[0].x, point2d[0].y), 1.7, Scalar(50, 255, 200), 3);
    
    // Mat rmat;
    // Rodrigues(rvec,rmat);
    // double yaw = std::atan2(rmat.at<double>(0,2),rmat.at<double>(2,2));
    // double pitch = -std::asin(rmat.at<double>(1, 2));
    // double roll = std::atan2(rmat.at<double>(1, 0), rmat.at<double>(1, 1));
    // tools::draw_text(img, fmt::format("euler angles:  yaw{: .2f} pitch{: .2f} roll{: .2f}", yaw*57.3, pitch*57.3, roll*57.3), cv::Point(10, 140), 1, cv::Scalar(0, 255, 255), 3);

}
*/


// 装甲板姿态
// 为rvec tvec firePoint进行赋值
void Armor::armorPose(Mat &img){
    // 对于我们而言，也就是 照片上 装甲板 4个点的坐标。
    std::vector<cv::Point2f> img_points{ 
        this->left.topPoint,
        this->right.topPoint,
        this->right.bottomPoint,
        this->left.bottomPoint };

    Mat rvec, tvec;
    solvePnP(object_points, img_points,camera_matrix ,distort_coeffs , rvec, tvec,false,SOLVEPNP_IPPE);
    // tools::draw_text(img, fmt::format("tvec:  x{: .2f} y{: .2f} z{: .3f}", tvec.at<double>(0),tvec.at<double>(1), tvec.at<double>(2)), cv::Point(10, 60), 1, cv::Scalar(0, 255, 255), 3);
    // tools::draw_text(img, fmt::format("rvec:  x{: .2f} y{: .2f} z{: .2f}", rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2)), cv::Point(10, 100), 1, cv::Scalar(0, 255, 255), 3);
    this->armorDistance = tvec.at<double>(2);// 距离
    this->rvec = rvec.clone();
    this->tvec = tvec.clone();

    // 1. 获取OpenCV相机坐标系下的坐标
    double x_cv = tvec.at<double>(0);  // 右X
    double y_cv = tvec.at<double>(1);  // 下Y  - 0.085 
    double z_cv = tvec.at<double>(2);  // 前Z

    // 3. 初始化firePoint
    vector<Point3f> point3d = {Point3f((float)x_cv, (float)y_cv, (float)z_cv)};
    vector<Point2f> point2d;
    projectPoints(point3d, Mat::zeros(3,1,CV_64F), Mat::zeros(3,1,CV_64F), 
                camera_matrix, distort_coeffs, point2d);
    this->firePoint = point2d[0];
    this->firePoint.x-=10;

}



// 截取装甲板 
void Armor::get_pattern(const Mat & bgr_img, const Lightbar & left,const Lightbar &right){

    // 延长灯条获得装甲板角点
    // 1.125 = 0.5 * armor_height / lightbar_length = 0.5 * 126mm / 56mm // 比例扩大
    
    // 使用一个收缩系数，比如0.2
    Point2f center_vector = (right.midPoint - left.midPoint) * 0.185;// 0.15 - 0.2 区间内比较好

    Point2f tl = (left.midPoint - left.top2bottom * 1.2) + center_vector;
    Point2f bl = (left.midPoint + left.top2bottom * 1.2) + center_vector;
    Point2f tr = (right.midPoint - right.top2bottom * 1.2) - center_vector;
    Point2f br = (right.midPoint + right.top2bottom * 1.2) - center_vector;

    // 直截取roi区域(优化)
    // Mat roiImg = bgr_img(Rect(tl, br)).clone();
    // imshow("roiImg",roiImg);

    // 透视变换
    vector<Point2f>corners(4);
    corners[0] = tl;  //左上角
    corners[1] = tr;  //右上角
    corners[2] = br;  //右下角
    corners[3] = bl;  //左下角

    // （调试显示）test
    // for (size_t i = 0; i < corners.size(); i++) {
    //     circle(bgr_img, corners[i], 5, Scalar(0, 255, 0), -1);
    //     putText(bgr_img, to_string(i + 1), corners[i],
    //         FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255), 1);
    // }

    Mat frame(400,400,CV_8UC3,Scalar(0,0,0));//固定尺寸

    vector<Point2f>corner_f(4);
    corner_f[0] = Point2f(0,0);                   //左上角
    corner_f[1] = Point2f(frame.cols,0);          //右上角
    corner_f[2] = Point2f(frame.cols,frame.rows); //右下角
    corner_f[3] = Point2f(0,frame.rows);          //左下角

    // 计算透视变换矩阵
    Mat&& M1 = getPerspectiveTransform(corners, corner_f);

    // 应用透视变换，输出尺寸与原图相同
    warpPerspective(bgr_img, frame, M1, frame.size(), INTER_LINEAR, BORDER_CONSTANT,Scalar(0,0,0));
    
    // 转bgr
    cvtColor(frame,frame,COLOR_BGR2GRAY);
    // threshold(frame,frame,17,255,THRESH_BINARY);// （被优化了）
    threshold(frame,frame,0,255,THRESH_BINARY | THRESH_OTSU);

    this->roiImg = frame.clone();
}

// 机器人判断

/*void Armor::Name_Dcide(){

    // 加载ONNX模型
    static cv::dnn::Net net;
    static bool modelLoaded = false;
    
    if (!modelLoaded) {
        try {
            net = cv::dnn::readNetFromONNX("/home/ljm123/Documents/vision01/rm_f/best.onnx");
            modelLoaded = true;
            std::cout << "模型加载成功!" << std::endl;
        } catch (const cv::Exception& e) {
            std::cerr << "无法加载模型: " << e.what() << std::endl;
            this->robotName = ArmorPatten::Name::black; // 如果模型加载失败，返回默认值
        }
    }

    Mat input = cv::Mat(48, 48, CV_8UC1, cv::Scalar(0)); // 目标是将任意尺寸的图像缩放到32×32的固定尺寸
    // 为了保持图像的长宽比不变，需要计算X和Y方向的缩放比例
    double x_scale = static_cast<double>(48) / this->roiImg.cols; // 横比例
    double y_scale = static_cast<double>(48) / this->roiImg.rows; // 列比例
    double scale = std::min(x_scale, y_scale); // 选择较小的缩放比例确保图像完全适应目标尺寸框内
    int h = static_cast<int>(this->roiImg.rows * scale); // 让图像中的列像素×比例
    int w = static_cast<int>(this->roiImg.cols * scale); // 让图像中的横像素×比例
    auto roi = cv::Rect(0, 0, w, h); // 生成截取的区域
    cv::resize(this->roiImg, input(roi), {w, h});
 
    // //保存图片（用于训练）
    // if(i<1500){
    //     cv::imwrite("/home/ljm123/Documents/vision01/rm_f/number/two_img/img"+to_string(i)+".png",input);
    //     i++;
    // }else{
    //     cout<<"over!!!"<<endl;
    // }

    // 预处理图像
    Mat inputBlob;
    
    // 将单通道图像转换为3通道图像
    Mat threeChannelImg;
    if (input.channels() == 1) {
        cvtColor(input, threeChannelImg, COLOR_GRAY2BGR);
    } else {
        threeChannelImg = input;
    }
    
    // 转换为浮点型并归一化到0-1范围
    Mat floatImg;
    threeChannelImg.convertTo(floatImg, CV_32F, 1.0/255.0);
    
    // 创建输入blob
    inputBlob = cv::dnn::blobFromImage(floatImg, 1.0, Size(48, 48), Scalar(0), true, false);


    // 设置网络输入
    net.setInput(inputBlob);
    
    // 前向传播
    Mat output = net.forward();
    
    // 解析输出结果
    Point classIdPoint;
    double confidence;
    minMaxLoc(output.reshape(1, 1), nullptr, &confidence, nullptr, &classIdPoint);
    int classId = classIdPoint.x;
    
    // 将分类ID映射到ArmorPatten::Name
    ArmorPatten::Name rbName;

        switch (classId) {
        case 0:
            rbName = ArmorPatten::Name::black; // 空
            break;
        case 1:
            rbName = ArmorPatten::Name::hero; // 英雄
            break;
        case 2:
            rbName = ArmorPatten::Name::sentry; // 哨兵
            break;
        case 3:
            rbName = ArmorPatten::Name::infantry; // 步兵
            break;
        default:
            rbName = ArmorPatten::Name::black; // 默认为空
            break;
        }
        // std::cout << "识别结果: " << classId << ", 置信度: " << confidence << std::endl;

    this->robotName = rbName;
}

*/